import rclpy
from rclpy.node import Node
from chapt4_interfaces.srv import FaceDetector 
from ament_index_python.packages import get_package_share_directory
from cv_bridge import CvBridge # 用于转换格式
import cv2
import face_recognition
import time
from rcl_interfaces.msg import SetParametersResult


class FaceDetectorClientNode(Node):
    def __init__(self):
        super().__init__('face_detect_client_node')
        self.bridge = CvBridge()
        self.defaut_image_path = get_package_share_directory('demo_python_service')+'/resource/test1.jpg'
        self.get_logger().info(f'人脸检测客户端启动')
        self.client = self.create_client(FaceDetector, '/face_detect')
        self.image = cv2.imread(self.defaut_image_path)

    def send_request(self):
        # 1、判断服务端是否在线
        while not self.client.wait_for_service(timeout_sec=1.0):
            self.get_logger().info('等待服务端启动')
        # 2、构造request请求
        request = FaceDetector.Request()
        request.image = self.bridge.cv2_to_imgmsg(self.image)
        # 3、发送请求并等待处理完成(异步处理，现在future并没有包含响应结果，需要等待服务端处理完成才会把结果放到future中)
        future = self.client.call_async(request)
        # 多线程ros情况下，while没有问题，但是现在单线程会导致阻塞，导致无法继续执行后续代码
        # while not future.done():
        #     time.sleep(1.0)

        # 在执行spin时，不断查询结果，直到future完成，才继续执行后续代码
        # rclpy.spin_until_future_complete(self, future)
        def result_callback(result_future):
            response = result_future.result()
            self.get_logger().info(f'检测到{response.number}张人脸, 耗时{response.use_time}秒')
            self.show_response(response)
        # 4、使用回调函数处理响应结果
        future.add_done_callback(result_callback)



    def show_response(self, response):
        for i in range(response.number):
            top = response.top[i]
            right = response.right[i]
            bottom = response.bottom[i]
            left = response.left[i]
            cv2.rectangle(self.image, (left, top), (right, bottom), (0, 0, 255), 2)

        cv2.imshow('face_detect', self.image)
        cv2.waitKey(0) # 也是阻塞的，会导致spin无法正常运行，等待按键按下，否则无法显示图像


def main(args=None):
    rclpy.init(args=args)
    node = FaceDetectorClientNode()
    node.send_request()
    rclpy.spin(node)
    rclpy.shutdown()